#ifndef COLORTRACKER_H_
#define COLORTRACKER_H_

#include "../Driver.h"
#include "../obstacles/RandomWalk.h"
#include "../../utilities/Constants.h"
#include "../../utilities/ObjPosition.h"

#include <string>
#include <std_msgs/Float32MultiArray.h>
#include <boost/thread.hpp>

using namespace std;
namespace std {
using boost::mutex;
using boost::condition_variable;
using boost::unique_lock;
using boost::thread;
}

#define STOPPING_DIST 0.55 // default distance (meters) to stop before object
#define ARM_RANGE 0.54 // maximal distance (meters) that the arm can reach to

/**
 * Moves robot towards object's current position: \n
 * given current object position, as published by \ref ../../VisionNode/VisionNode.h,
 * this object instructs the correct driving distance
 */
class ColorTracker {

private:


	Driver* m_driver; // object to move the robot
	RandomWalk* m_rand_walk; // used to choose random turn
	double m_speed; // linear driving speed (meter/second)
	double m_backup_speed; // stores given speed, used after slowing down towards the object
	short m_no_object; // if not 0, then object is not visible and should consider obstacles indicated by laser sensor

	// ros management objects
	ros::NodeHandle m_node_handler;
	ros::Subscriber m_subscriber;
	ros::Rate* m_loop_rate;
	std_msgs::String m_msg_to_pub_sensors;
	ros::Publisher m_sensors_publisher;
	ros::Subscriber sub_sensors_fl;

	// used for synchronized access to data
	std::mutex m_mutex;

	float m_right_bin; // average distance of right section of laser scan
	float m_center_bin; // average distance of central section of laser scan
	float m_left_bin; // average distance of left section of laser scan
	float m_range[MAX_RANGE_SIZE]; // current sample from front laser

	/**
	 * Calculates turn angle (radians) needed to correct robot's orientation,
	 * in order to align the robot according to object's position. \n
	 * Attention: Kinect uses negative values to indicate left, but negative
	 * turn angles for rover indicate right turn
	 * @param object's horizontal deviation (cm) from Kinect
	 * @param distance (cm) to the object
	 * @return turn angle to align the robot according to the object
	 */
	float calcHorizontalOrientation(float xCord, float dist);

    /**
     * Reads last value updated by the callback \ref positionSetter(). \n
     * Reading is synchronized because trying to read from the same varaible
     * \ref positionSetter() is writing to
     * @return object's current position
     */
	ObjPosition safeRead();

    /**
     * Parses object's position out of a message publishes by vision node
     * @param[in] msg message holding object's position
     * @param[out] x object's real-world x-axis coordinate
     * @param[out] y object's real-world y-axis coordinate
     * @param[out] dist distance to object
     * @param[out] size object's size in image
     */
	void parseMsg(string msg, float& x, float& y, float& dist, float& size);

	/**
	 * Returns x's sign. If x is 0, its sign is set as positive
	 * @param x value to check its sign
	 * @return 1 if x is positive or 0, -1 if x is negative
	 */
	short getSign(float x);

	/**
	 * Aligns robot according to object's position.
	 * Then, drives towards the object
	 * @param cur_pos object's current real-world position
	 */
	void moveToObject(const ObjPosition& cur_pos);

	/**
	 * Checks if robot is close enough to the object. If so, no more alignments will
     * take place. \n
     * Uses also the distance to the object as sampled at previous frame, in order
     * to ignore false data from the Kinect
	 * @param dist current distance to the object
	 * @param prev_dist distance to the object as sample at previous frame
	 * @return True if is close enough to the object, False if not
	 */
	bool isArrivingToObj(const float& dist, const float& prev_dist);

	/**
	 * Calculates average of values in array in given interval
	 * @param range array of values, as sampled from front laser sensor
	 * @param start start of interval
	 * @param end end of interval
	 * @return average of values in input array in given interval
	 */
	float avgElements(const std_msgs::Float32MultiArray::ConstPtr& range,const int& start, const int& end) const;

	/**
	 * Calculates average of values in array in given interval
	 * @param range array of values
	 * @param start start of interval
	 * @param end end of interval
	 * @return average of values in input array in given interval
	 */
	float avgElements(const float range[], const int& start, const int& end) const;

	/**
	 * Reads safely (with mutex) the distances (in meters) from laser of each side. \n
	 * Each distance is a member that is updated in callback from \ref SensorNode
	 * @param[out] right average value of right side of laser range
	 * @param[out] center average value of center section of laser range
	 * @param[out] left average value of left side of laser range
	 */
	void getBins(float& right, float& center, float& left);

	/**
	 * Updates members that stores distances from robot
	 * @param right average value of right side of laser range
	 * @param center average value of center section of laser range
	 * @param left average value of left side of laser range
	 */
	void setBins(const float right, const float center, const float left);

	/**
	 * Safely converts RosMessage multi-array to float array. \n
	 * Updates member m_range.
	 * @param range array of distances from laser
	 */
	void setRange(const std_msgs::Float32MultiArray::ConstPtr& range);

	/**
	 * If an obstacle is too close to front laser's left, right or middle
	 * area (average value in one of these bins is too small),
	 * then the robot is too close to an obstacle and should stop
	 * @return True if should perform emegency stop, False if not
	 */
	bool isEmergencyStop();

	/**
	 * Stops code execution for \ref TIME_FOR_TURN seconds.
	 * Works like regular sleep, but every TIME_FOR_TURN/1000 seconds runs ros::spinOnce
	 * to refresh data from callbacks
	 */
	void sleepAndWatch(int time_for_turn); /*in microseconds*/

	/**
	 * Checks if robot's final position is not aligned correctly towards the object. \n
	 * The alignment is wrong if object's horizontal deviation is too extreme, hence out
	 * of Kinect's effective distance
	 * @param dist distance towards the object (negative value means out of effective distance)
	 * @return True if alignment is wrong, False if not
	 */
	bool isWrongAlignment(const float& dist) const;

	/**
	 * Called if the robot has reached the object but the object is out
	 * of arm's reach or out of Kinect's effective distance. \n
	 * The method drives the robot backwards until it is far enough to
	 * track the object again
	 */
	void recovery();

	/**
	 * Should perform recovery if object's final position is too close (out of Kinect's
	 * effective distance) or if distance to the object is greater than recovery_dist
	 * @param final_pos object's position as sampled after robot's complete stop
	 * @param recovery_dist threshold distance- any greater final distance results in \ref recovery()
	 * @return True if should perform recovery, False if not
	 */
	bool shouldRecover(const ObjPosition& final_pos, const float& recovery_dist) const;

public:

    /**
     * Constructor- creates a new tracker object
     * @param driver pointer to object handling the robot's moves
     * @param rand_walk pointer to object managing random walk
     * @param speed linear speed (meter/second) in which tracking is performed
     */
	ColorTracker(Driver* driver, RandomWalk* rand_walk, double speed);

    /**
     * Destructor- stops the robot and cancels subscription to front laser sensor
     */
	virtual ~ColorTracker();

    /**
	 * Tracks object until robot has reached object (that is, close enough so the arm can hold it).
	 * If robot's final position exceeds Kinect's effective distance (that is, either too close or
	 * horizontal deviation to object is too extreme), it turns around and tries again. \n
	 * If reaches a wall and object is not visible, moves to the clearer direction (right or left)
	 * or to a random direction if both are clear (uses rand_walk). \n
	 * If reaches an obstacle that is too close, then stops
	 * @param stop_dist when the robot reaches distance (cm) from the object, it stops. default value
	 * allows the arm to reach the object
	 * @param recvery_dist if robot has stopped and is distance towards the object is more than recovery_dist,
	 * it turns around and tries again
	 * @return object's position sampled after complete stop
	 */
	ObjPosition track(float stop_dist = STOPPING_DIST, float recvery_dist = ARM_RANGE);

    /**
     * Callback for topic published by vision node, which informs
     * object's current position
     * @param msg topic message holding object's current position
     */
	void positionSetter(const std_msgs::String::ConstPtr& msg);	

    /**
	 * Subscribes or de-subscribes to range sampled by front laser sensor
	 * @param shouldSubscribe True indicates to establish subsctiption, False indicates to cancel subscription
	 */
	void subscribeToLaser(bool shouldSubscribe);

	// Laser (Sensors node) callback
    /**
	 * Callback to receive the range published by the sensors node
	 * @param range range sampled by front laser sensor
	 */
	void frontLaserCB(const std_msgs::Float32MultiArray::ConstPtr& range) ;
};

#endif /* COLORTRACKER_H_ */
